#include "iostream"
#include <ros/ros.h>
#include <vector>
#include "../include/objectPoint.h"
#include "../include/featureObjectExtractor.h"
#include "../include/LSfeatureObject.h"
#include "../include/define.h"
#include "../include/CandidateExtractor.h"
#include <sensor_msgs/LaserScan.h>
#include <geometry_msgs/Pose2D.h>
#include <hri_msgs/Torsos.h>
#include <hri_msgs/Torso.h>
//#define __OPENMP


#ifdef __OPENMP
#include<omp.h>
#endif

using namespace std;
using namespace hri_msgs;

/*
*   create By: MJ Namazifar(MJ.Namazifar@qiau.ac.ir)
*                R O B O T
*
*                  X
*                  |
*                  |
*                  |
*   -Y <-----------|---------->Y
*   -90            0          90
*
*
*
*              URG or UBG
*       Laser start form right to left
*
*       127 - 640 ---------> 180 ( 90  , -90  )
*       44  - 725 ---------> 240 ( 120 , -120 )
*                    UTM
*
*/


class robina_torsoDetector_LRF
{
public:
    robina_torsoDetector_LRF():nh("~")
    {
        nh.param<std::string>("topic",scan_topic,"/scan_ubg");
        int type;
        nh.param("laser_type",type,0);
        devType = (LRF_DEVICE)type;
        setupLRF(devType);
        sampleCount=0;
    }
public:
    void run();
    void laser_sub(const sensor_msgs::LaserScanConstPtr&);
    void poseHuman_pub(vector<Candidate *> pobj);
    void setupLRF(LRF_DEVICE dev);
    void doTorsoDetection(vector <objectPoint> objps);
private:
    double toRadian(double value);
    void appearanceCandadiates(vector<Candidate *> candidates);
    void appendCandidatesforSampling(vector<Candidate *> cands);
     vector<Candidate> getValidcandidates();
private:
    ros::Subscriber laserData_sub;
    ros::Publisher  legsPos2D_pub;
    int startIndex , endIndex;
    int sampleCount;
    double startAngle ;
    double endAngle;
    double reso;
    int pointCount;
    int scanCount;
    int SensorFrontStep;
    vector <objectPoint> ObjectPoints;
    featureObjectExtractor featureExtract;
    CandidateExtractor torsoExtract;
    ros::NodeHandle nh;
    std::string scan_topic;
    LRF_DEVICE devType;
    vector<Candidate> Validcandidates;
};

double robina_torsoDetector_LRF::toRadian(double value)
{
    return value * M_PI / 180;
}
void robina_torsoDetector_LRF::setupLRF(LRF_DEVICE dev)
{
    string str = (dev == UTM)? "UTM":"UBG";
    cout<<" Initialized with "<< str <<" Laser Range Finder Device "<<endl;
    startIndex = (dev == UTM)   ? 180 : 128;
    endIndex   =   (dev == UTM) ? 900 : 640;
    pointCount = endIndex - startIndex;
    reso       = 360.0 / ( (dev == UTM) ? 1440 : 1024);
    startAngle = -(pointCount ) * reso / 2.0;
    endAngle   = -startAngle;
    SensorFrontStep = (dev == UTM) ? 540 : 384;

    double t = startAngle;
    cout<<" count point : "<<pointCount<<" startAngle : "<<startAngle<<" EndAngle : "<<endAngle;
    cout<<"\n ---------------------------------------\n";
    for (int i = 0; i < pointCount; i++, t+= reso)
    {
        ObjectPoints.push_back( objectPoint( toRadian(t) , i) );
    }
}


void robina_torsoDetector_LRF::run()
{

    laserData_sub = nh.subscribe<sensor_msgs::LaserScan>("/scan_ubg",1,
                                         (boost::function < void(const sensor_msgs::LaserScanConstPtr&)>) boost::bind( &robina_torsoDetector_LRF::laser_sub, this, _1 ));
    legsPos2D_pub = nh.advertise<hri_msgs::Torsos>("/torsos",1);

}
void robina_torsoDetector_LRF::laser_sub(const sensor_msgs::LaserScanConstPtr &msg)
{
#ifdef __OPENMP
    omp_set_num_threads(4);

#pragma omp parallel for shared(msg)
#endif
    if(legsPos2D_pub.getNumSubscribers()==0)
    {
        ros::Rate(10).sleep();
        return;
    }
    for(int i= 0 ; i < msg->ranges.size() ; i++)
    {
            ObjectPoints[i].setR ( msg->ranges[i] * 100);
    }

    doTorsoDetection(ObjectPoints);
} 

void robina_torsoDetector_LRF::doTorsoDetection(vector<objectPoint> objps)
{
    featureExtract.setDiagonal(65,14);//(45,18)
    //(45,14); good
    vector<LSfeatureObject> objects = featureExtract.doExtract( objps , 70 , 25  );//30,8
    //cout<<"\n object count : "<< objects.size();
    torsoExtract.setObjects(objects);
    torsoExtract.doExtractor( 0 , 0 );
    vector<Candidate*> cands = torsoExtract.getCandidates();
    poseHuman_pub(cands);
    //   appearanceCandadiates(cands);
    /*
    for(vector<Candidate*>::iterator it = cands.begin();it != cands.end();it++)
    {
        delete *it;
      //  it = cands.erase(it);
    }*/
//    sampleCount++;
//   if(sampleCount > 8)
//    {
//        vector<Candidate> valid_candides = getValidcandidates();
//        poseHuman_pub(valid_candides);
//        sampleCount = 0;
//        Validcandidates.clear();
//        Validcandidates = valid_candides;
//    }
 }

void robina_torsoDetector_LRF::poseHuman_pub(vector<Candidate*> candidates)
{
    Torso tors;
    Torsos torsos;
    for(vector<Candidate*>::iterator i = candidates.begin();i != candidates.end();i++)
    {

        tors.human_arm_left_pos.position.x = (*i)->object1_.getStartPoint().getX()/100.0;
        tors.human_arm_left_pos.position.y = (*i)->object1_.getStartPoint().getY()/100.0;

        tors.center_of_torso.position.x =  (*i)->object1_.getMiddlePoint().getX()/100.0;
        tors.center_of_torso.position.y =  (*i)->object1_.getMiddlePoint().getY()/100.0;

        tors.human_arm_right_pos.position.x = (*i)->object1_.getEndPoint().getX()/100.0;
        tors.human_arm_right_pos.position.y = (*i)->object1_.getEndPoint().getY()/100.0;

        tors.confidence.data = (*i)->object1_.confidenceFactor;
        torsos.torsos.push_back(tors);
        delete *i;
    }
    legsPos2D_pub.publish(torsos);
}

void robina_torsoDetector_LRF::appearanceCandadiates(vector<Candidate *> candidates)
{

    if(candidates.size() <= 0)
    {
        appendCandidatesforSampling(candidates);
    }
    else
    {
        // match candidate
        for(vector<Candidate *>::iterator it=candidates.begin();it!= candidates.end();)
        {
            bool flag = false;
            for(vector<Candidate>::iterator itt = Validcandidates.begin(); itt != Validcandidates.end();itt++)
            {
                if( (itt->matchCandidates( **it )== true) && ( !itt->IsMatch() ) )
                {
                    itt->SetMatch(true);
                    itt->updateCandidate( **it );
                    itt->appearanceCount++;
                    //cout<<"\n itt->appearanceCount : "<<itt->appearanceCount;
                    delete *it;
                    it = candidates.erase(it);
                    flag = true;
                    break;
                }
            }
            if(!flag) it++;
        }
        //append candidate when can't match
        if(  candidates.size() > 0 )
        {
            cout<<"\n append candidates when can't match.";
            appendCandidatesforSampling(candidates);
        }
        for(vector<Candidate>::iterator itt = Validcandidates.begin(); itt != Validcandidates.end();itt++)
        {
            itt->SetMatch(false);
        }
    }
}
void robina_torsoDetector_LRF::appendCandidatesforSampling(vector<Candidate *> cands)
{
    for(vector<Candidate*>::iterator it = cands.begin();it != cands.end();)
    {
        Candidate can = **it;
        can.appearanceCount++;
        Validcandidates.push_back(can);
        delete *it;
        it = cands.erase(it);
    }
    cands.clear();
}
vector<Candidate> robina_torsoDetector_LRF::getValidcandidates()
{

    for(vector<Candidate>::iterator itt = Validcandidates.begin(); itt != Validcandidates.end();)
    {
        if( itt->appearanceCount < 4)
        {
            itt = Validcandidates.erase(itt);
        }
        else
        {   itt++;
        }
    }
    for(vector<Candidate>::iterator itt = Validcandidates.begin(); itt != Validcandidates.end();itt++)
    itt->appearanceCount = 0;
    return Validcandidates;
}

int main(int argc, char *argv[])
{
    ros::init(argc,argv,"roboina_torsoDetector_LRF");

    robina_torsoDetector_LRF legDetect;
        cout<<"\n Torso DETECTION ==> start"<<endl;

    legDetect.run();
//    ros::Rate loop(1000);
//    while(ros::ok())
//    {
//        ros::spinOnce();
//        loop.sleep();
//    }
    ros::spin();
    cout<<"\n Torso DETECTION ==> finish"<<endl;
    return 0;
}
